/**
 *=============================================================================
 *  @copyright (C)      2025CCUT_1024lab_Damocles
 *
 *  @file    alg_slope.c
 *  @brief   斜坡函数, 用于速度规划等
 *
 *  @note    使用方法
 *
 *  @attention 注意事项
 *-----------------------------------------------------------------------------
 *  @version    @author    @date    @note
 *  V1.0.0      Cherno    250808    done
 *
 *==============================================================================
 */

/*==============================Includes======================================*/
#include "alg_slope.h"
/*==============================PrivateMacros=================================*/

/*==============================PrivateTypes==================================*/

template <typename Type>
Type Math_Abs(Type x)
{
	return ((x > 0) ? x : -x);
}
/*==============================PrivateVariables==============================*/

/*==============================PrivateFunctionDeclarations===================*/

/*==============================FunctionPrototypes============================*/

/**
 * @brief 初始化
 *
 * @param _increase_value 增长最大幅度
 * @param _decrease_value 降低最大幅度
 */
void Slope::init(float _increase_value, float _decrease_value)
{
	increase_value = _increase_value;
	decrease_value = _decrease_value;
}

void Slope::slope_calc()
{
	if (now > 0.0f) {
		if (target > now) {
			// 正值加速
			if (Math_Abs(now - target) > increase_value) {
				out += increase_value;
			} else {
				out = target;
			}
		} else if (target < now) {
			// 正值减速
			if (Math_Abs(now - target) > decrease_value) {
				out -= decrease_value;
			} else {
				out = target;
			}
		}
	} else if (now < 0.0f) {
		if (target < now) {
			// 负值加速
			if (Math_Abs(now - target) > increase_value) {
				out -= increase_value;
			} else {
				out = target;
			}
		} else if (target > now) {
			// 负值减速
			if (Math_Abs(now - target) > decrease_value) {
				out += decrease_value;
			} else {
				out = target;
			}
		}
	} else {
		if (target > now) {
			// 0值正加速
			if (Math_Abs(now - target) > increase_value) {
				out += increase_value;
			} else {
				out = target;
			}
		} else if (target < now) {
			// 0值负加速
			if (Math_Abs(now - target) > increase_value) {
				out -= increase_value;
			} else {
				out = target;
			}
		}
	}
	now = out;
}

/*========================COPYRIGHT(C) CCUT-DAMOCLES==========================*/

